﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.CompilerServices;

namespace Futaba.SBus.Telemetry
{
	/// <summary>
	/// Implementation of <see cref="ITelemetryParser"/>
	/// </summary>
	public class TelemetryParser : NotifyPropertyChangedBase, ITelemetryParser
	{

		#region Singleton

		private static ITelemetryParser _singletonTelemetryParser;
		private static readonly object TelemetryParserSingletonLock = new object();

		/// <summary>
		/// Singleton instance of the <see cref="ITelemetryParser"/> default implementation
		/// </summary>
		public static ITelemetryParser DefaultInstance
		{
			get
			{
				if (_singletonTelemetryParser == null)
				{
					// ReSharper disable PossibleMultipleWriteAccessInDoubleCheckLocking
					lock (TelemetryParserSingletonLock)
					{
						if (_singletonTelemetryParser == null)
							_singletonTelemetryParser = new TelemetryParser();
					}
					// ReSharper restore PossibleMultipleWriteAccessInDoubleCheckLocking
				}
				return _singletonTelemetryParser;
			}
		}

		#endregion

		#region ITelemetryParser

		private int _internalSequenceErrors;
		/// <summary>
		/// Returns the amount of Sequence-Errors which occured
		/// </summary>
		/// <remarks>
		/// Sequence information is only returned, when reading from the TX's port, as the TX
		/// will also return telemetry data, if it is currently not receiving telemetry from the model
		/// </remarks>
		/// <seealso cref="Reset"/>
		public int SequenceErrors
		{
			get { return _internalSequenceErrors; }
			protected set
			{
				if (Equals(_internalSequenceErrors, value))
					return;
				_internalSequenceErrors = value;
				RaisePropertyChanged(); //RaisePropertyChanged("SequenceErrors");
			}
		}

		private bool _internalTxTelemetryPassthroughMode;
		/// <summary>
		/// Set by the <see cref="ISbusParsingService"/>, if channel-data is also available
		/// </summary>
		/// <remarks>
		/// When reading from the TX's data-port, telemetry data is sent a bit differently.
		/// The sequence byte is only sent from the TX, and the TX will also stream telemetry data, 
		/// even if it's not receiving newer values from the RX. It will also send ZEROs for many
		/// slots every second packet (probably because it doesn't receive telemetry as fast as it
		/// streams it to the data port)
		/// </remarks>
		public bool TxTelemetryPassthroughMode
		{
			get { return _internalTxTelemetryPassthroughMode; }
			set
			{
				if (Equals(_internalTxTelemetryPassthroughMode, value))
					return;
				_internalTxTelemetryPassthroughMode = value;
				RaisePropertyChanged(); //RaisePropertyChanged("TxTelemetryPassthroughMode");
			}
		}

		private SlotValueParser _internalValueParser;
		/// <summary>
		/// The value parser used to extract the actual value from the payload of the slot.
		/// If not set, then <see cref="SlotValueParser.DefaultInstance"/> is used
		/// </summary>
		public SlotValueParser ValueParser
		{
			get { return _internalValueParser ?? SlotValueParser.DefaultInstance; }
			set
			{
				if (Equals(_internalValueParser, value))
					return;
				_internalValueParser = value;
				RaisePropertyChanged(); //RaisePropertyChanged("ValueParser");
			}
		}

		/// <summary>
		/// Raised if a telemetry-packet has been received with an unexpected sequence
		/// </summary>
		/// <seealso cref="SequenceErrors"/>
		public event EventHandler OutOfSequence;

		private void RaiseOutOfSequence()
		{
#if DEBUG
			Debug.WriteLine("Raising OutOfSequence", GetType().Name);
#endif
			if (OutOfSequence == null)
				return;

			OutOfSequence.BeginInvoke(this, EventArgs.Empty, r => HandleEventInvocation(r, OutOfSequence.EndInvoke), OutOfSequence);
		}

		/// <summary>
		/// Raised when values for a slot are received, but the slot is not in the <see cref="ItemsCollection"/>
		/// </summary>
		public event EventHandler<SlotIdType> UnknownSlotReceived;

		private void RaiseUnknownSlotReceived(SlotIdType slotId)
		{
#if DEBUG
			Debug.WriteLine("Raising UnknownSlotReceived", GetType().Name);
#endif
			if (UnknownSlotReceived == null)
				return;

			UnknownSlotReceived.BeginInvoke(this, slotId, r => HandleEventInvocation(r, UnknownSlotReceived.EndInvoke), OutOfSequence);
		}

		#endregion

		#region ISbusParser
		private Readers.ISbusStreamReader _internalReader;
		/// <summary>
		/// The reader which reads from the actual stream
		/// </summary>
		public Readers.ISbusStreamReader Reader
		{
			get { return _internalReader; }
			set
			{
				if (Equals(_internalReader, value))
					return;
				_internalReader = value;
				RaisePropertyChanged(); //RaisePropertyChanged("Reader");
			}
		}

		private int _internalBytesSkipped;
		/// <summary>
		/// Returns the amount of bytes which were not considered part of a valid package
		/// </summary>
		/// <seealso cref="Reset"/>
		public int BytesSkipped
		{
			get { return _internalBytesSkipped; }
			protected internal set
			{
				if (Equals(_internalBytesSkipped, value))
					return;
				_internalBytesSkipped = value;
				RaisePropertyChanged(); //RaisePropertyChanged("BytesSkipped");
			}
		}

		private int _internalPacketsReceived;
		/// <summary>
		/// Returns the amount of packets which were successfully received and processed
		/// </summary>
		/// <seealso cref="Reset"/>
		public int PacketsReceived
		{
			get { return _internalPacketsReceived; }
			protected set
			{
				if (Equals(_internalPacketsReceived, value))
					return;
				_internalPacketsReceived = value;
				RaisePropertyChanged(); //RaisePropertyChanged("PacketsReceived");
			}
		}

		private int _internalPacketErrors;
		/// <summary>
		/// Returns the amount of packets which were not fully processed
		/// </summary>
		/// <seealso cref="Reset"/>
		public int PacketErrors
		{
			get { return _internalPacketErrors; }
			protected set
			{
				if (Equals(_internalPacketErrors, value))
					return;
				_internalPacketErrors = value;
				RaisePropertyChanged(); //RaisePropertyChanged("PacketErrors");
			}
		}


		private IList<ISlot> _internalItemsCollection;
		/// <summary>
		/// Holds the data of the parser
		/// </summary>
		public IList<ISlot> ItemsCollection
		{
			get { return _internalItemsCollection; }
			set
			{
				if (Equals(_internalItemsCollection, value))
					return;
				_internalItemsCollection = value;
				ResetWarnedSlots();
				RaisePropertyChanged(); //RaisePropertyChanged("ItemsCollection");
			}
		}

		private bool _internalCancellationPending;
		/// <summary>
		/// Set to True from <see cref="ISbusParsingService"/>, if the current operation is to be aborted
		/// </summary>
		public bool CancellationPending
		{
			get { return _internalCancellationPending; }
			set
			{
				if (Equals(_internalCancellationPending, value))
					return;
				_internalCancellationPending = value;
				RaisePropertyChanged(); //RaisePropertyChanged("CancellationPending");
			}
		}

		/// <summary>
		/// Raised when an element in the <see cref="ItemsCollection"/> has changed
		/// </summary>
		public event EventHandler<ISlot> ItemUpdated;

		private void RaiseItemUpdated(ISlot slot, byte[] data)
		{
#if DEBUG
			var dataText = BitConverter.ToString(data);
			Debug.WriteLine(string.Format("Raising ItemUpdated ({0}) (data: {1})", slot, dataText), GetType().Name);
#endif
			if (ItemUpdated == null)
				return;

			ItemUpdated.BeginInvoke(this, slot, r => HandleEventInvocation(r, ItemUpdated.EndInvoke), ItemUpdated);
		}

		/// <summary>
		/// Raised (once per data-packet) when at least one element in the <see cref="ItemsCollection"/> has changed
		/// </summary>
		public event EventHandler ItemsUpdated;

		private void RaiseItemsUpdated()
		{
#if DEBUG
			Debug.WriteLine("Raising ItemsUpdated", GetType().Name);
#endif
			if (ItemsUpdated == null)
				return;

			ItemsUpdated.BeginInvoke(this, EventArgs.Empty, r => HandleEventInvocation(r, ItemsUpdated.EndInvoke), ItemsUpdated);
		}

		/// <summary>
		/// Raised when a valid S.BUS packet has been received and parsed
		/// (regardless wether any channel has changed)
		/// </summary>
		public event EventHandler DataReceived;

		private void RaiseDataReceived()
		{
#if DEBUG
			Debug.WriteLine("Raising DataReceived", GetType().Name);
#endif
			if (DataReceived == null)
				return;

			DataReceived.BeginInvoke(this, EventArgs.Empty, r => HandleEventInvocation(r, DataReceived.EndInvoke), DataReceived);
		}

		/// <summary>
		/// Detect, if this type of data is being supplied in the current stream
		/// </summary>
		/// <returns>True, if this instance can parse data from the current <see cref="Reader"/></returns>
		public bool DetectData(byte[] data)
		{
			return TryDetectTelemetryPackage(data);
		}

		/// <summary>
		/// Read and parse data from the stream
		/// </summary>
		/// <returns>True, if a valid packet was detected, else False</returns>
		public bool ReadPacket()
		{
			var success = TryParseTelemetry();
			if (success)
				PacketsReceived++;
			else if (!CancellationPending)
				PacketErrors++;
			return success;
		}

		/// <summary>
		/// Reset the counters
		/// </summary>
		/// <seealso cref="BytesSkipped"/>
		/// <seealso cref="PacketsReceived"/>
		/// <seealso cref="PacketErrors"/>
		public void Reset()
		{
			if (ItemsCollection != null)
			{
				foreach (var slot in ItemsCollection)
					slot.CurrentValue = null;
			}

			CancellationPending = false;
			SequenceErrors = BytesSkipped = PacketsReceived = PacketErrors = 0;
		}

		#endregion

		#region detection

		readonly byte[][] _firstSlots =
		{
			new [] { (byte)SlotIdType.Slot0, (byte)SlotIdType.Slot1 }, 
			new [] { (byte)SlotIdType.Slot8, (byte)SlotIdType.Slot9 }, 
			new [] { (byte)SlotIdType.Slot16, (byte)SlotIdType.Slot17 },
			new [] { (byte)SlotIdType.Slot24, (byte)SlotIdType.Slot25 }
		};
		private bool TryDetectTelemetryPackage(byte[] data)
		{
			// ReSharper disable once LoopCanBeConvertedToQuery
			foreach (var slotPair in _firstSlots)
			{
				var firstIdx = Array.IndexOf(data, slotPair[0]);
				if (firstIdx < 0)
					continue;
				if ((firstIdx + 3) < data.Length && data[firstIdx + 3] == slotPair[1])
				{
					SkipToTelemetryMessageStart();
					return true;
				}
			}
			return false;
		}

		private int SkipToTelemetryMessageStart()
		{
			var lookFor = _firstSlots.Select(s => s[0]).ToArray();
			int bytesSkipped = 0;
			while (true)
			{
				if (CancellationPending)
					return bytesSkipped;
				var value = Reader.Peek();
				if (!value.HasValue)
					continue;
				if (lookFor.Any(s => s == value.Value))
					return bytesSkipped;
				Reader.ReadByte();
				bytesSkipped++;
			}
		}
		#endregion

		#region TelemetryParsing

		/// <summary>
		/// Tries to read and parse the telemetry packet from the stream
		/// </summary>
		/// <returns>true, if a valid packet has been processed, else false</returns>
		protected virtual bool TryParseTelemetry()
		{
			byte seqByte = 0;
			if (TxTelemetryPassthroughMode)
			{
				seqByte = ReadAndAnalyseSeqByte();
				if (seqByte == 0)
				{
					LoggingService.Log.TraceEvent(TraceEventType.Warning, LoggingService.DataOutOfSyncEventId, "No Sequence-Byte detected!");
					return false;
				}
			}
			else
			{
				var bytesSkipped = SkipToTelemetryMessageStart();
				if (bytesSkipped > 0)
					BytesSkipped += bytesSkipped;
				seqByte = GetNextSeqByte(seqByte);
			}

			return TryParseTelemetry(seqByte);
		}

		private bool TryParseTelemetry(byte seqByte)
		{
			//var telemetryBytes = new List<byte>(26);
			var processedByteCount = 0;
			var anyTelemetryChanged = false;
			var isFirstSlotInMsg = true;
			var nowUtc = DateTime.UtcNow;
			var slotsProcessed = 0;
			while (slotsProcessed < 8)
			{
				if (CancellationPending)
					return false;

				var peek = Reader.Peek();
				if (!peek.HasValue)
					continue;

				//begin of channels message, should not happen
				if (peek.Value == 0x0f)
				{
					BytesSkipped += processedByteCount; //telemetryBytes.Count;
					return false;
				}

				byte? value;
				bool logByte = true;
				//futaba doesn't always send in ID for this slot?!
				//on home TX/RX it does, on one at work it doesn't
				//RX R6308SBT doesn't send it aswell
				if (isFirstSlotInMsg && new[] { TelemetrySequences[1], TelemetrySequences[5] }.Contains(seqByte))
				{
					logByte = Reader.ReadIfMatch((byte)SlotIdType.Slot8);
					value = (byte)SlotIdType.Slot8;
				}
				else
				{
					value = Reader.ReadByte();
				}

				if (!value.HasValue)
				{
					BytesSkipped += processedByteCount; //telemetryBytes.Count;
					return false;
				}
				var slotValue = value.Value;
				isFirstSlotInMsg = false;

				//fake byte, don't pretend it came via stream
				if (logByte)
				{
					processedByteCount++;
					//telemetryBytes.Add(slotValue);
				}

				byte? subSlotId = null;
				while (!subSlotId.HasValue)
				{
					subSlotId = Reader.Peek();
				}

				ISlot slot = GetSlot(slotValue, subSlotId.Value);
				if (slot == null)
				{
					//empty slots are also sent
					var value1 = Reader.ReadByte();
					var value2 = Reader.ReadByte();
					if (value1 != 0 || value2 != 0)
					{
						var slotId = GetSlotId(slotValue);
						if (slotId.HasValue)
							RaiseUnknownSlotReceived(slotId.Value);
						else
						{
							LoggingService.Log.TraceEvent(TraceEventType.Warning, LoggingService.DataOutOfSyncEventId, "Value {0} should be a slotId, but is not", slotValue);
							RaiseOutOfSequence();
						}
					}
					slotsProcessed++;
					continue;
				}

				try
				{
					var slotByteCount = slot.Parameter.ConsumesSlots * 2;
					var data = new List<byte>(slotByteCount);
					for (var curSlot = 0; curSlot < slot.Parameter.ConsumesSlots; curSlot++)
					{
						var slotBytes = 0;
						bool hasSlotIdByteBeenConsumed = false;
						while (slotBytes < 2)
						{
							var v = Reader.ReadByte();
							if (!v.HasValue)
								continue;

							processedByteCount++;
							//telemetryBytes.Add(v.Value);
							if (curSlot > 0 && !hasSlotIdByteBeenConsumed)
							{
								hasSlotIdByteBeenConsumed = true;
								continue; //skip slotId byte
							}
							data.Add(v.Value);
							slotBytes++;
						}
					}

					//all slots send old values, if no new values are received, except receiverV
					//but if only telemetry data is received (not via TX, but via separate receiver), then no data at all is sent,
					//so we can raise the event every time
					if (!TxTelemetryPassthroughMode || (slot.SlotId == SlotIdType.Slot0 && data[0] != 0x0 && data[1] != 0x0))
					{
						RaiseDataReceived();
					}

					var hasSlotChanged = ParseValue(slot, data.ToArray(), nowUtc);
					//Debug.WriteLine("{0} slots (starting at {1}) parsed by {2} ({3} values changed)", parser.ConsumesSlots, slot.Slot, parser.GetType().Name, changedSlots.Count());
					slotsProcessed += slot.Parameter.ConsumesSlots;
					if (hasSlotChanged)
					{
						RaiseItemUpdated(slot, data.ToArray());
						anyTelemetryChanged = true;
					}
				}
#if DEBUG
				catch (Exception ex)
				{
					LoggingService.LogException(ex, LoggingService.ParsingErrorEventId, "Unexpected error reading slots. {0}", ex.Message);
					if (Debugger.IsAttached)
						Debugger.Break();
				}
#else
				catch (Exception) { }
#endif
			}
			if (anyTelemetryChanged)
				RaiseItemsUpdated();
			return true;
		}

		private bool ParseValue(ISlot slot, byte[] data, DateTime nowUtc)
		{
			var parser = ValueParser;
			var ctx = slot.ParseContext;
			ctx.Data = data;
			ctx.UpdateFromSlot(slot);

			try
			{
				ctx.ProcessedCount++;
				var newValue = parser.Parse(ctx);

				// ReSharper disable once CompareOfFloatsByEqualityOperator
				if (newValue == 0)
				{
					ctx.ConsecutiveZeroValueCount++;
					ctx.ZeroValueCount++;
				}
				else
				{
					ctx.ConsecutiveZeroValueCount = 0;
				}

				//0 values are ok, if they are received constantly, but not, if they are sent every second packet
				// ReSharper disable once CompareOfFloatsByEqualityOperator
				if (newValue == 0 && ctx.ConsecutiveZeroValueCount < 2)
				{
					var packetToZeroRatio = ctx.ZeroValueRatio;
					if (packetToZeroRatio > 0.4 && packetToZeroRatio < 0.6)
					{
						//Debug.WriteLine("{0} packetToZeroRatio={1}", slot, packetToZeroRatio);
						return false;
					}
				}

				bool slotChanged = AssignValue(slot, newValue, nowUtc);
				return slotChanged;
			}
			catch (Exception ex)
			{
				var dataText = BitConverter.ToString(data);
				LoggingService.LogException(ex, LoggingService.ParsingErrorEventId,
					"Error parsing Value of slot {0} (Data: {1}). {2}", slot.SlotId, dataText, ex.Message);
				return false;
			}
		}

		/// <summary>
		/// Assigns the value to the slot, if is has changed
		/// </summary>
		/// <returns>true, if the value has changed, false if not</returns>
		protected bool AssignValue(ISlot slot, double newValue, DateTime nowUtc)
		{
			slot.ReceivedUtc = nowUtc;

			//don't change and raise events, if nothing changed for this slot
			// ReSharper disable CompareOfFloatsByEqualityOperator
			if (slot.CurrentValue == newValue)
				return false;
			// ReSharper restore CompareOfFloatsByEqualityOperator

			slot.CurrentValue = newValue;
			return true;
		}

		readonly int[] _seqOffset = new int[4];
		private byte GetNextSeqByte(byte seqByte)
		{
			if (seqByte != 0)
			{
				for (int i = 0; i < TelemetrySequences.Length; i++)
				{
					var t = TelemetrySequences[i];
					if (seqByte != t)
						continue;
					if ((i + 1) < TelemetrySequences.Length)
						return TelemetrySequences[i + 1];
				}
			}
			else
			{
				//synchronize sequence with slot-blocks
				var peek = Reader.Peek();
				switch (peek)
				{
					case (byte)SlotIdType.Slot0:
						_seqOffset[0] = _seqOffset[0] == 0 ? 4 : 0;
						return TelemetrySequences[0 + _seqOffset[0]];
					case (byte)SlotIdType.Slot8:
						_seqOffset[1] = _seqOffset[1] == 0 ? 4 : 0;
						return TelemetrySequences[1 + _seqOffset[1]];
					case (byte)SlotIdType.Slot16:
						_seqOffset[2] = _seqOffset[2] == 0 ? 4 : 0;
						return TelemetrySequences[2 + _seqOffset[2]];
					case (byte)SlotIdType.Slot24:
						_seqOffset[3] = _seqOffset[3] == 0 ? 4 : 0;
						return TelemetrySequences[3 + _seqOffset[3]];
				}
			}
			return TelemetrySequences[0];
		}

		private byte ReadAndAnalyseSeqByte()
		{
			byte seqByte = 0; //TelemetrySequences
			// ReSharper disable once LoopCanBeConvertedToQuery
			for (int i = 0; i < TelemetrySequences.Length; i++)
			{
				var t = TelemetrySequences[i];
				if (!Reader.ReadIfMatch(t))
					continue;
				seqByte = t;
				//telemetryBytes.Add(seqByte);
				break;
			}
			if (seqByte != 0)
				AlertIfOuOfSequence(seqByte);
			return seqByte;
		}

		private readonly bool[] _warnedSlots = new bool[0xff]; //see SlotIdType

		private void ResetWarnedSlots()
		{
			for (var i = 0; i < 0xff; i++)
				_warnedSlots[i] = false;
		}

		private ISlot GetSlot(byte slotValue, byte subSlot)
		{
			var slotId = GetSlotId(slotValue);
			if (!slotId.HasValue)
				return null;
			var slots = ItemsCollection.Where(s => s.SlotId == slotId.Value).ToArray();
			switch (slots.Count())
			{
				case 0:
					if (!_warnedSlots[slotValue])
					{
						LoggingService.Log.TraceEvent(TraceEventType.Warning, LoggingService.InvalidSlotEventId,
							"Could not find a slot with SlotID {0}", slotId);
						_warnedSlots[slotValue] = true;
					}
					return null;
				case 1:
					return slots.Single();
				default:
					var slot = slots.SingleOrDefault(s => s.Parameter.SubSlotId.HasValue && s.Parameter.SubSlotId == subSlot);
					if (slot == null && !_warnedSlots[slotValue])
					{
						LoggingService.Log.TraceEvent(TraceEventType.Warning, LoggingService.InvalidSlotEventId,
							"Could not find a slot with SlotID {0} and SubSlot {1}", slotId, subSlot);
						_warnedSlots[slotValue] = true;
					}
					return slot;
			}
		}

		private static SlotIdType? GetSlotId(byte slotValue)
		{
			if (!Enum.IsDefined(typeof(SlotIdType), slotValue))
				return null;
			var slotId = (SlotIdType)slotValue;
			return slotId;
		}

		static readonly byte[] TelemetrySequences = { 0x04, 0x14, 0x24, 0x34, 0x0c, 0x1c, 0x2c, 0x3c };
		int _lastTelemetrySeq;
		private void AlertIfOuOfSequence(byte curSeq)
		{
			var lastIdx = Array.IndexOf(TelemetrySequences, _lastTelemetrySeq);
			var curIdx = Array.IndexOf(TelemetrySequences, curSeq);
			var seqInfo = string.Format("Sequence Error! {0} >> {1}", _lastTelemetrySeq, curSeq);
			_lastTelemetrySeq = curSeq;

			if (lastIdx < 0)
				return;

			//on private TX, I only get first part of sequence
			if (curIdx == 0 && lastIdx == 3)
				return;

			if (curIdx == lastIdx + 1)
				return;

			if (curIdx == 0 && lastIdx == (TelemetrySequences.Length - 1))
				return;

#if DEBUG
			Debug.WriteLine(seqInfo);
#endif
			SequenceErrors++;
			RaiseOutOfSequence();
		}
		#endregion

		#region HandleEventInvocation

		private void HandleEventInvocation(IAsyncResult result, Action<IAsyncResult> endInvoke, [CallerMemberName]string eventName = null)
		{
			try
			{
				endInvoke(result);
				//if (result.AsyncState is EventHandler)
				//	((EventHandler)result.AsyncState).EndInvoke(result);
			}
			catch (Exception ex)
			{
				// ReSharper disable once PossibleNullReferenceException
				var evName = (eventName ?? "").StartsWith("Raise") ? eventName.Substring(5) : eventName;
				LoggingService.LogException(ex, LoggingService.ParsingErrorEventId,
					"Error on Event-Handler for event {0}.{1}. {2}",
					GetType().FullName, evName, ex.ToString());
			}
		}

		#endregion

	}
}
